/*
 * rt_model.c
 *
 * Created: 29.03.2012 07:54:09
 *  Author: KZM
 */ 
#include "rt_model.h"
#include <stdlib.h>
#include "adc.h"

struct model_in_t {
	int MotorCurrent1;
	int MotorCurrent2;
	int BattVoltage;
	int BattCurrent;
	int Throttle;
	int torque1;
	int torque2;
	int rpm1;
	int rpm2;
	int Motor1Voltage;
	int Motor2Voltage;
} model_in;

struct model_out_t {
	int MotorPwm1;
	int MotorPwm2;
} model_out;

void WritePWM(void){
	SetMotor1PWM(model_out.MotorPwm1);
	SetMotor2PWM(model_out.MotorPwm2);
	
	model_in.Motor1Voltage= model_in.BattVoltage * (model_out.MotorPwm1 / 1024.0);
	model_in.Motor2Voltage= model_in.BattVoltage * (model_out.MotorPwm2 / 1024.0);
}
#define MOTOR1_I_LL 1
#define MOTOR2_I_LL 1
#define MOTOR1_C 0.0672091255
#define MOTOR2_C 0.0672091255
#define MOTOR1_R 0.30163385
#define MOTOR2_R 0.30163385

int CalcMotorTorque(void){
	model_in.torque1=(model_in.MotorCurrent1 - MOTOR1_I_LL * (model_in.rpm1/3350) ) * MOTOR1_C;
	model_in.torque2=(model_in.MotorCurrent2 - MOTOR2_I_LL * (model_in.rpm2/3350) ) * MOTOR2_C;
}
int CalcMotorRPM(void){
	//(self.U-self.I*self.R)/(2*self.C*math.pi)
	model_in.rpm1 = (model_in.Motor1Voltage - model_in.MotorCurrent1 * MOTOR1_R) / (2 * MOTOR1_C * 3.14);
	model_in.rpm2 = (model_in.Motor2Voltage - model_in.MotorCurrent2 * MOTOR2_R) / (2 * MOTOR2_C * 3.14);
}

void ReadAnalog(void){
	model_in.MotorCurrent1 = GetMotor1CurrentmA();
	model_in.MotorCurrent2 = GetMotor2CurrentmA();
	model_in.BattCurrent = GetBatteryCurrentmA();
	model_in.BattVoltage = GetBatteryVoltagemV();
	model_in.Throttle = GetThrottle();
}
int max(int a, int b){
	if (a>b)
		return a;
	else
		return b;
}
#define MAX_CURRENT_FWD 15.0
#define MAX_CURRENT_RWD 15.0
int16_t absolut(int16_t a);

int16_t absolut(int16_t a){
	if (a < 0)
		a*=-1;
	return a;
}
void ControllStep(void){
	static int PWM_ist=0;
	int16_t MaxMotorCurrent=0;
	float stell=0;
	//int16_t temp;
	
	MaxMotorCurrent = max( model_in.MotorCurrent1, model_in.MotorCurrent2 );
	stell += ((model_in.Throttle-PWM_ist)/100.0);
	
	if (model_in.Throttle >= PWM_ist){
		stell *= ( ( MAX_CURRENT_FWD * ( model_in.Throttle / 1000.0 ) ) - ( MaxMotorCurrent/30.0) );
	}
	else{
        stell *= ( ( MAX_CURRENT_FWD ) + ( MaxMotorCurrent / 30.0 ) );
	}
	
	if (max(model_in.MotorCurrent1, model_in.MotorCurrent2) > 20000){
		stell += 1;
	}	
	PWM_ist += stell;
	
	if (PWM_ist > 1024){
		PWM_ist=1024;
	}
	
	if (PWM_ist < 0){
		PWM_ist=0;
	}
	
	model_out.MotorPwm1=PWM_ist;
	model_out.MotorPwm2=PWM_ist;
}

void rt_step(void){
	static char buffer[255];
	uint16_t Throttle;
	static uint16_t i=0;
	ReadAnalog();
	ControllStep();
	WritePWM();
	i+=model_out.MotorPwm1;
	if (i>0x3FE){
		i=0;
		if (PORTB &  0x80)
			PORTB &= ~0x80;
		else
			PORTB |= 0x80;
	}		
	sprintf(buffer,"Throttle: %d\n",model_out.MotorPwm1);
	UartSenden(buffer);
} 